#include "planner.h"
#include "log/zuclog.h"
#include <cmath>
#include <string.h>
using namespace axis;

#define PLANNER_DEBUG(fmt, ...) zuclog_debug("PLANNER", fmt, ##__VA_ARGS__)
#define PLANNER_INFO(fmt, ...) zuclog_info("PLANNER", fmt, ##__VA_ARGS__)

void axis::Planner::init_traj_planner(double planner_interval, double traj_interval)
{
    if (planner_interval > traj_interval)
    {
        // PLANNER_DEBUG("Error occur, planner_interval : %lf is larger than traj_interval :%lf\n", planner_interval, traj_interval);
        planner_interval = traj_interval;
    }
    planner_interval_ = planner_interval;
    traj_interval_ = traj_interval;

    planner_mutex_.lock();
    target_.is_real = true;
    target_.is_planned = false;
    pre_target_.is_real = true;
    pre_target_.is_planned = false;
    planner_mutex_.unlock();
}

void axis::Planner::reset()
{
    planner_mutex_.lock();
    target_.is_planned = false;
    target_.is_real = true;
    planner_mutex_.unlock();
}

// traj 线程调用
void axis::Planner::push_pos(TrajPoint& pos, bool is_jump)
{
    // PLANNER_DEBUG("push_pos!\n");
    planner_mutex_.lock();
    if (!is_traj_start)
    {
        // 刚启动
        is_traj_start = true;
        target_ = pos;
        target_.is_real = true;
        target_.is_planned = false;
        target_.err = 0;
    }
    else
    {
        if (!target_.is_real)
        {
            // PLANNER_DEBUG("not real!\n");

            if (is_jump)
            {
                target_ = pos;
                target_.err = 0;
                pre_target_ = target_;
            }
            else
            {
                // 这个点是预测出来的，现在真实点进来了，需要替换调，并计算误差, 保持target中的是上一点的预测位置，这样下次插补时就不会产生位置跳变
                target_.is_real = true;
                target_.vel = pos.vel;
                target_.acc = pos.acc;
                target_.jerk = pos.jerk;
                target_.err = pos.pos - target_.pos;  // 预测点和真实点之间的误差，会随着下一个真实点的到来，自动被补偿掉
            }
        }
        else
        {
            // PLANNER_DEBUG("real!\n");
            if (is_jump)
            {
                pre_target_ = pos;  // jump, 这里的插补不能由上一个指令插
                target_ = pos;
            }
            else
            {
                pre_target_ = target_;
                target_ = pos;
            }
            target_.is_planned = false;
        }
    }
    planner_mutex_.unlock();
}

// rtbus 线程调用
int axis::Planner::do_interp(InterpType interp_type, PredictType predict_type)
{
    // PLANNER_DEBUG("do_interp!\n");
    planner_mutex_.lock();
    if (target_.is_planned)  // 前面已经用过了，说明没有新点进来，预测一个点
    {
        // PLANNER_DEBUG("do predict!\n");
        pre_target_ = target_;
        target_ = do_predict(predict_type, target_);  // 先预测
        target_.is_real = false;
        lost_count_++;
    }
    else
    {
        // PLANNER_DEBUG("do plan!\n");
        lost_count_ = 0;
    }
    target_.is_planned = true;
    planner_mutex_.unlock();

    double s0 = pre_target_.pos;
    double se = target_.pos;
    double v0 = pre_target_.vel;
    double ve = target_.vel;
    double a0 = pre_target_.acc;
    double ae = target_.acc;
    double j0 = pre_target_.jerk;
    double je = target_.jerk;
    double dt = traj_interval_;  // 外部轨迹规划时间间隔
    do_plan(interp_type, s0, se, v0, ve, a0, ae, j0, je, dt);
    return 0;
}

int axis::Planner::calc_s(double ts, double& pos)
{
    ts *= 1.0;
    pos = (poly_para_[0] + poly_para_[1] * ts + poly_para_[2] * pow(ts, 2) + poly_para_[3] * pow(ts, 3) + poly_para_[4] * pow(ts, 4) +
           poly_para_[5] * pow(ts, 5) + poly_para_[6] * pow(ts, 6) + poly_para_[7] * pow(ts, 7));
    return 0;
}

void axis::Planner::get_poly_para(double* para) const
{
    for (int i = 0; i < 9; i++) { para[i] = poly_para_[i]; }
}

int axis::Planner::do_plan(InterpType interp_type, double s0, double se, double v0, double ve, double a0, double ae, double j0, double je, double dt)
{
    for (int i = 0; i < 9; i++) { poly_para_[i] = 0.0; }
    switch (interp_type)
    {
    case INTERP_POLY3:
        do_plan_3th(s0, se, v0, ve, a0, ae, j0, je, traj_interval_);
        break;
    case INTERP_POLY5:
        do_plan_5th(s0, se, v0, ve, a0, ae, j0, je, traj_interval_);
        break;
    case INTERP_POLY7:
        do_plan_7th(s0, se, v0, ve, a0, ae, j0, je, traj_interval_);
        break;
    case INTERP_LINEAR:
    default:
        do_plan_linear(s0, se, v0, ve, a0, ae, j0, je, traj_interval_);
        break;
    }
    return 0;
}

TrajPoint axis::Planner::do_predict(PredictType predict_type, const TrajPoint& point_start)
{
    double pos = point_start.pos;
    double vel = 0;
    double acc = 0;
    double jerk = 0;
    switch (predict_type)
    {
    case PREDICT_HOLD_ACC:
        vel = point_start.vel;
        acc = point_start.acc;
        break;
    case PREDICT_HOLD_JERK:
        vel = point_start.vel;
        acc = point_start.acc;
        jerk = point_start.jerk;
    case PREDICT_HOLD_VEL:
    default:
        vel = point_start.vel;
        break;
    }
    return calc_predict_point(pos, vel, acc, jerk);
}

TrajPoint axis::Planner::get_planner_target()
{
    planner_mutex_.lock();
    TrajPoint res = target_;
    planner_mutex_.unlock();
    return res;
}

TrajPoint axis::Planner::get_planner_pre_target()
{
    planner_mutex_.lock();
    TrajPoint res = pre_target_;
    planner_mutex_.unlock();
    return res;
}

int axis::Planner::get_traj_lost_count()
{
    planner_mutex_.lock();
    int res = lost_count_;
    planner_mutex_.unlock();
    return res;
}

TrajPoint axis::Planner::calc_predict_point(double pos, double vel, double acc, double jerk)
{
    acc += jerk * traj_interval_;
    vel += acc * traj_interval_;
    pos += vel * traj_interval_;
    return TrajPoint{pos, vel, acc, jerk};
}

int axis::Planner::do_plan_linear(double s0, double se, double v0, double ve, double a0, double ae, double j0, double je, double dt)
{
    dt *= 1.0;
    poly_para_[0] = s0;
    poly_para_[1] = (double)(se - s0) / dt;
    return 0;
}

int axis::Planner::do_plan_3th(double s0, double se, double v0, double ve, double a0, double ae, double j0, double je, double te)
{
    te *= 1.0;
    poly_para_[0] = s0;
    poly_para_[1] = v0;
    poly_para_[2] = -(3.0 * s0 - 3.0 * se + ve * te + 2.0 * v0 * te) / pow(te, 2);
    poly_para_[3] = (2.0 * s0 - 2.0 * se + ve * te + v0 * te) / pow(te, 3);
    return 0;
}

int axis::Planner::do_plan_5th(double s0, double se, double v0, double ve, double a0, double ae, double j0, double je, double te)
{
    te *= 1.0;
    poly_para_[0] = s0;
    poly_para_[1] = v0;
    poly_para_[2] = a0 / 2.0;
    poly_para_[3] = -(20.0 * s0 - 20.0 * se + 12.0 * te * v0 + 8.0 * te * ve + 3.0 * a0 * pow(te, 2) - ae * pow(te, 2)) / (2.0 * pow(te, 3));
    poly_para_[4] = (30.0 * s0 - 30.0 * se + 16.0 * te * v0 + 14.0 * te * ve + 3.0 * a0 * pow(te, 2) - 2.0 * ae * pow(te, 2)) / (2.0 * pow(te, 4));
    poly_para_[5] = -(12.0 * s0 - 12.0 * se + 6.0 * te * v0 + 6.0 * te * ve + a0 * pow(te, 2) - ae * pow(te, 2)) / (2.0 * pow(te, 5));
    return 0;
}

int axis::Planner::do_plan_7th(double s0, double se, double v0, double ve, double a0, double ae, double j0, double je, double te)
{
    te *= 1.0;
    poly_para_[0] = s0;
    poly_para_[1] = v0;
    poly_para_[2] = a0 / 2.0;
    poly_para_[3] = j0 / 6.0;
    poly_para_[4] = -(1050.0 * se - 1050.0 * s0 - 270.0 * ve * te - 1260.0 * se * te - 780.0 * v0 * te + 1260.0 * s0 * te + 15.0 * ae * pow(te, 2) -
                      30.0 * ae * pow(te, 3) + 360.0 * ve * pow(te, 2) + je * pow(te, 3) - 270.0 * a0 * pow(te, 2) + 300.0 * a0 * pow(te, 3) +
                      900.0 * v0 * pow(te, 2) - 56.0 * j0 * pow(te, 3) + 60.0 * j0 * pow(te, 4)) /
                    (6.0 * pow(te, 4) * (30.0 * te - 29));
    poly_para_[5] = (168.0 * s0 - 168.0 * se + 78.0 * ve * te + 90.0 * v0 * te - 14.0 * ae * pow(te, 2) + je * pow(te, 3) + 20.0 * a0 * pow(te, 2) +
                     2.0 * j0 * pow(te, 3)) /
                    (2.0 * pow(te, 5) * (30.0 * te - 29.0));
    poly_para_[6] = -(2940.0 * s0 - 2940.0 * se + 1104.0 * ve * te + 2520.0 * se * te + 1836.0 * v0 * te - 2520.0 * s0 * te - 129.0 * ae * pow(te, 2) +
                      90.0 * ae * pow(te, 3) - 900.0 * ve * pow(te, 2) + 3.0 * je * pow(te, 3) + 495.0 * a0 * pow(te, 2) - 450.0 * a0 * pow(te, 3) -
                      1620.0 * v0 * pow(te, 2) + 64.0 * j0 * pow(te, 3) - 60.0 * j0 * pow(te, 4)) /
                    (6.0 * pow(te, 6) * (30.0 * te - 29.0));
    poly_para_[7] = -(1560.0 * s0 - 1560.0 * se + 600.0 * ve * te + 1440.0 * se * te + 960.0 * v0 * te - 1440.0 * s0 * te - 72.0 * ae * pow(te, 2) +
                      60.0 * ae * pow(te, 3) - 540.0 * ve * pow(te, 2) + je * pow(te, 3) + 252.0 * a0 * pow(te, 2) - 240.0 * a0 * pow(te, 3) -
                      900.0 * v0 * pow(te, 2) + 31.0 * j0 * pow(te, 3) - 30.0 * j0 * pow(te, 4)) /
                    (6.0 * pow(te, 3) * (-30.0 * pow(te, 5) + 29.0 * pow(te, 4)));
    return 0;
}
